#include "sub.hpp"

namespace mars_rover
{
  namespace sub_yuan
  {
    SubscriberNode::SubscriberNode(const rclcpp::NodeOptions &options) : Node("sub_node", options)
    {
      std::cout << "start..." << std::endl;
      source_frame_ = this->declare_parameter<std::string>("source_frame", "source_frame");
      target_frame_ = this->declare_parameter<std::string>("target_frame", "target_frame");

    //   static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
      pointcloud_subscriber_ = this->create_subscription<sensor_msgs::msg::PointCloud2>("pointcloud_raw", 1, 
      std::bind(&SubscriberNode::topic_points_callback, this, std::placeholders::_1));

      image_subscriber_ = this->create_subscription<sensor_msgs::msg::Image>("image_raw", 1, 
      std::bind(&SubscriberNode::topic_image_callback, this, std::placeholders::_1));

      pose_subscriber_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("pose_raw", 1, 
      std::bind(&SubscriberNode::topic_poses_callback, this, std::placeholders::_1));


      tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
      tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

      pub_thread_.reset(new std::thread(&SubscriberNode::tf_sub, this));
      // timer_ = this->create_wall_timer(
      // 1s, std::bind(&SubscriberNode::tf_sub, this));  //error: unable to find numeric literal operator ‘operator""s’
    }

    void SubscriberNode::topic_image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {   
        image_ = *msg;
        cv::Mat img;
        cv_bridge::CvImagePtr cv_ptr;
        cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
        cv_ptr->image.copyTo(img);
        
        cv::imshow("sub_Image",img);
        cv::waitKey(3);
    }

    void SubscriberNode::topic_points_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {   
        pointcloud_ = *msg;
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
        pcl::fromROSMsg(pointcloud_, *cloud);
        pcl::io::savePCDFileASCII ("sub_pc.pcd", *cloud);
 
 
        // RCLCPP_INFO(this->get_logger(), "points_size(%d,%d)",msg->height,msg->width);
        
    }

    void SubscriberNode::topic_poses_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
    {   
        pose_ = *msg;
        RCLCPP_INFO(this->get_logger(), "poses_x_y_z:(%f,%f,%f)", 
        pose_.pose.position.x, pose_.pose.position.y, pose_.pose.position.z);
        
    }

    void SubscriberNode::tf_sub()
    { 
      sleep(1);
      rclcpp::Rate rate(1.0);
      while (rclcpp::ok()){
        geometry_msgs::msg::PoseStamped temp;

        try {
          transform_stamp_ = tf_buffer_->lookupTransform(
              target_frame_, source_frame_,
              tf2::TimePointZero);  // get newest tf

          tf2::doTransform(pose_, temp, transform_stamp_);

          RCLCPP_INFO(this->get_logger(), "before_tf:(%f,%f,%f)", 
            pose_.pose.position.x, pose_.pose.position.y, pose_.pose.position.z);
          
          RCLCPP_INFO(this->get_logger(), "after_tf:(%f,%f,%f)", 
            temp.pose.position.x, temp.pose.position.y, temp.pose.position.z);

        } catch (const tf2::TransformException & ex) {
          RCLCPP_INFO(
            this->get_logger(), "no tf found");
          continue;
        }
        
        rate.sleep();
      }
    }

  }
}

RCLCPP_COMPONENTS_REGISTER_NODE(mars_rover::sub_yuan::SubscriberNode)
